#include <mc_core/robot_model/robot_model_base.h>

ModelBase::ModelBase(string configFile){
    configFile_ = configFile;
    parserRobotModel();
}


// 解析机器人模型
void ModelBase::parserRobotModel(){
    if(configFile_.empty()){
        ROS_ERROR("[ModelBase | parserRobotModel] config file is empty!");
        return;
    }
    YAML::Node yaml_config = file_manager::ReadYamlFile(configFile_);
    if(yaml_config.IsNull()){
        ROS_ERROR("[ModelBase | parserRobotModel] yaml config file is null!");
        return;
    }
    if(yaml_config["robot_name"]){
        robotModel_.robot_name = yaml_config["robot_name"].as<string>();
    }
    if(yaml_config["chassis_model"]){
        robotModel_.chassis_model = yaml_config["chassis_model"].as<string>();
    }


    // polygon 多边形
    if(robotModel_.chassis_model == "polygon"){
        if(yaml_config["robot_footprints"]){
            for (const auto& footprint : yaml_config["robot_footprints"]){
                geometry_msgs::Point32 point;
                point.x = footprint[0].as<float>();
                point.y = footprint[1].as<float>();
                point.z = 0.0f;  // Assuming 2D robot footprints
                robotModel_.robot_footprints.emplace_back(point);
            }
            // 计算内切圆和外接圆半径
            calculateIncircleRadius(robotModel_.robot_footprints,
                robotModel_.circumscribed_circle_radius, robotModel_.inscribed_circle_radius);
        }
    }

    // circle 圆形
    if(robotModel_.chassis_model == "circle"){
        if(yaml_config["robot_radius"]){
            robotModel_.robot_radius = yaml_config["robot_radius"].as<float>();
        }
        // 计算内切圆和外接圆半径
        robotModel_.inscribed_circle_radius = robotModel_.robot_radius;
        robotModel_.circumscribed_circle_radius = robotModel_.robot_radius;
    }

}

// 获取机器人模型
RobotModel ModelBase::getRobotModel(){
    return robotModel_;
}


// 获取多边形轮廓
visualization_msgs::Marker ModelBase::PolygonMarker(const std::string& frame_id, vector<geometry_msgs::Point32> polygon, string ns){
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;  
    marker.header.stamp = ros::Time::now();
    marker.ns = ns;  
    marker.id = 0;  
    marker.type = visualization_msgs::Marker::LINE_STRIP;  
    marker.action = visualization_msgs::Marker::ADD;
    marker.scale.x = 0.01;  // Line width

    marker.color.a = 0.7;  // Opaque color
    marker.color.r = 0.0;  // Red
    marker.color.g = 0.0;  // Green
    marker.color.b = 1.0;  // Blue

    for(const auto& point : polygon) {
        geometry_msgs::Point p;
        p.x = point.x;
        p.y = point.y;
        p.z = 0.05;  
        marker.points.emplace_back(p);
    }

    if(polygon.size() > 1) {
        marker.points.emplace_back(marker.points[0]);  
    }

    marker.pose.position.x = 0.0;
    marker.pose.position.y = 0.0;
    marker.pose.position.z = 0.0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
    return marker;
}

// 获取多边形轮廓
geometry_msgs::PolygonStamped ModelBase::toPolygonStamped(const std::string& frame_id){
    geometry_msgs::PolygonStamped polygon;
    polygon.header.frame_id = frame_id;
    polygon.header.stamp = ros::Time::now();
    for (const auto& point : robotModel_.robot_footprints) {
        geometry_msgs::Point32 p;
        p.x = point.x;
        p.y = point.y;
        polygon.polygon.points.emplace_back(p);
    }
    return polygon;
}

// 获取圆形轮廓
visualization_msgs::Marker ModelBase::CircleMarker(const std::string& frame_id, float radius, string ns) {
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;  
    marker.header.stamp = ros::Time::now();
    marker.ns = ns;  
    marker.id = 0;  
    marker.type = visualization_msgs::Marker::CYLINDER;;  
    marker.action = visualization_msgs::Marker::ADD;
    marker.scale.x = radius * 2;  // Diameter
    marker.scale.y = radius * 2;  // Diameter
    marker.scale.z = 0.01;

    marker.color.a = 0.4;  // Opaque color
    marker.color.r = 1.0;  // Red
    marker.color.g = 0.0;  // Green
    marker.color.b = 0.0;  // Blue

    marker.pose.position.x = 0.0;
    marker.pose.position.y = 0.0;
    marker.pose.position.z = 0.0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
    return marker;
}


// 获取机器人模型
visualization_msgs::MarkerArray ModelBase::getRobotModelMarkerArray(const std::string& frame_id){
    visualization_msgs::MarkerArray marker_array;
    if(robotModel_.chassis_model == "circle"){
        visualization_msgs::Marker robot_circle_marker = 
            CircleMarker(frame_id, robotModel_.robot_radius, "robot_radius");
        marker_array.markers.emplace_back(robot_circle_marker);
    }
    if(robotModel_.chassis_model == "polygon"){
        visualization_msgs::Marker robot_polygon_marker = 
            PolygonMarker(frame_id, robotModel_.robot_footprints, "robot_polygon");
        marker_array.markers.emplace_back(robot_polygon_marker);

        visualization_msgs::Marker inscribed_circle_marker = 
            CircleMarker(frame_id, robotModel_.inscribed_circle_radius, "inscribed_circle");
        marker_array.markers.emplace_back(inscribed_circle_marker);

        visualization_msgs::Marker circumscribed_circle_marker = 
            CircleMarker(frame_id, robotModel_.circumscribed_circle_radius, "circumscribed_circle");
        marker_array.markers.emplace_back(circumscribed_circle_marker);
    }
    return marker_array;
}


// 计算点到原点的距离
float ModelBase::distanceToOrigin(const geometry_msgs::Point32& point){
    return std::sqrt(point.x * point.x + point.y * point.y);
}

// 计算点到线段的垂直距离（点到线段的最短距离）
float ModelBase::pointToLineDistance(const geometry_msgs::Point32& p, const geometry_msgs::Point32& p1, const geometry_msgs::Point32& p2) {
    // 线段 p1->p2，点 p 到线段的距离
    float num = std::fabs((p2.y - p1.y) * p.x - (p2.x - p1.x) * p.y + p2.x * p1.y - p2.y * p1.x);
    float den = std::sqrt((p2.y - p1.y) * (p2.y - p1.y) + (p2.x - p1.x) * (p2.x - p1.x));
    return num / den;
}

// 计算最大内切圆半径和最小内切圆半径
void ModelBase::calculateIncircleRadius(const std::vector<geometry_msgs::Point32>& robot_footprints, float& max_radius, float& min_radius) {
    // 初始化最大和最小内接圆半径
    max_radius = std::numeric_limits<float>::min();
    min_radius = std::numeric_limits<float>::max();

    // 遍历每个顶点，计算质心（原点）到该顶点的距离，并计算质心到每条边的距离
    for (size_t i = 0; i < robot_footprints.size(); ++i) {
        size_t next = (i + 1) % robot_footprints.size();  // 循环连接最后一个顶点到第一个顶点
        const auto& p = robot_footprints[i];
        const auto& p_next = robot_footprints[next];

        // 计算顶点到原点的距离
        float dist_to_origin = distanceToOrigin(p);
        max_radius = std::max(max_radius, dist_to_origin);  // 最大半径
        min_radius = std::min(min_radius, dist_to_origin);  // 最小半径

        // 计算原点到边（p, p_next）的垂直距离
        geometry_msgs::Point32 origin_point;
        float dist_to_edge = pointToLineDistance(origin_point, p, p_next);
        max_radius = std::max(max_radius, dist_to_edge);  // 最大半径
        min_radius = std::min(min_radius, dist_to_edge);  // 最小半径
    }
}



